﻿/////////////////////////////////////////////////////////////////////////
//
// This module contains code to do a basic green screen.
//
// Copyright © Microsoft Corporation.  All rights reserved.  
// This code is licensed under the terms of the 
// Microsoft Kinect for Windows SDK (Beta) from Microsoft Research 
// License Agreement: http://research.microsoft.com/KinectSDK-ToU
//
/////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Coding4Fun.Kinect.Wpf;
using Microsoft.Research.Kinect.Nui; 

namespace WorkingWithDepthData
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        public MainWindow()
        {
            InitializeComponent();
        }

        //Kinect Runtime
        Runtime nui = new Runtime(); 

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {         
            //UseDepthAndPlayerIndex and UseSkeletalTracking
            nui.Initialize(RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking);

            //register for event
            nui.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_DepthFrameReady);

            //DepthAndPlayerIndex ImageType
            nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, 
                ImageType.DepthAndPlayerIndex); 

        }

        void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            //Convert depth information for a pixel into color information
            byte[] ColoredBytes = GenerateColoredBytes(e.ImageFrame); 

            //create an image based on returned colors
            
            PlanarImage image = e.ImageFrame.Image;
            image1.Source = BitmapSource.Create(image.Width, image.Height, 96, 96, PixelFormats.Bgr32, null, 
                ColoredBytes, image.Width * PixelFormats.Bgr32.BitsPerPixel/ 8); 
        }

        private byte[] GenerateColoredBytes(ImageFrame imageFrame)
        {
            
            int height = imageFrame.Image.Height;
            int width = imageFrame.Image.Width;

            //Depth data for each pixel
            Byte[] depthData = imageFrame.Image.Bits; 


            //colorFrame contains color information for all pixels in image
            //Height x Width x 4 (Red, Green, Blue, empty byte)
            Byte[] colorFrame = new byte[imageFrame.Image.Height * imageFrame.Image.Width * 4];

            //Bgr32  - Blue, Green, Red, empty byte
            //Bgra32 - Blue, Green, Red, transparency 
            //You must set transparency for Bgra as .NET defaults a byte to 0 = fully transparent

            //hardcoded locations to Blue, Green, Red (BGR) index positions       
            const int BlueIndex = 0;
            const int GreenIndex = 1;
            const int RedIndex = 2;
            int leastDistance = -1;
            var leastDepthIndex = -1;

            var depthIndex = 0;
            var minX = -1; 
            var minY = -1;
            for (var y = 0; y < height; y++)
            {                
                var heightOffset = y * width;

                for (var x = 0; x < width; x++)
                {
                    var index = ((width - x - 1) + heightOffset) * 4;

                    //var distance = GetDistance(depthData[depthIndex], depthData[depthIndex + 1]);
                    var distance = GetDistanceWithPlayerIndex(depthData[depthIndex], depthData[depthIndex + 1]);
                   
                    //if (distance <= 900)
                    //{                

                    //    //we are very close
                    //    colorFrame[index + BlueIndex] = 255;
                    //    colorFrame[index + GreenIndex] = 0;
                    //    colorFrame[index + RedIndex] = 0;
                    //}
                    //else if (distance > 900 && distance < 2000)
                    //{
                    //    //we are a bit further away
                    //    colorFrame[index + BlueIndex] = 0;
                    //    colorFrame[index + GreenIndex] = 255;
                    //    colorFrame[index + RedIndex] = 0;
                    //}
                    //else if (distance > 2000)
                    //{
                    //    //we are the farthest
                    //    colorFrame[index + BlueIndex] = 0;
                    //    colorFrame[index + GreenIndex] = 0;
                    //    colorFrame[index + RedIndex] = 255;
                    //}


                    ////equal coloring for monochromatic histogram
                    //var intensity = CalculateIntensityFromDepth(distance);
                    //colorFrame[index + BlueIndex] = intensity;
                    //colorFrame[index + GreenIndex] = intensity;
                    //colorFrame[index + RedIndex] = intensity;

                    //Color a player
                    if (GetPlayerIndex(depthData[depthIndex]) > 0)
                    {

                        if (leastDistance == -1 || distance <= leastDistance)
                        {
                            leastDistance = distance;
                            //save this depth - index
                            leastDepthIndex = index;
                            minX = x; minY = y;
                        }
                        //we are the farthest
                        colorFrame[index + BlueIndex] = 0;
                        colorFrame[index + GreenIndex] = (byte)((255 - distance)% 255);
                        colorFrame[index + RedIndex] = 0;
                    }

                    //jump two bytes at a time
                    depthIndex += 2;
                }
            }
            if (minX!=-1 && minY!=-1)
            {
                lblLeastDist.Content = "Dist: " + leastDistance + "\n ("+ minX+","+ minY +")";
                //MessageBox.Show("least depth distance" + leastDepthIndex);
                var idx = ((width - minX - 1) + minY * width) * 4;
                colorFrame[idx + BlueIndex] = 0;
                colorFrame[idx + GreenIndex] = 0;
                colorFrame[idx + RedIndex] = 255;
            }
            return colorFrame;
        }

        private static int GetPlayerIndex(byte firstFrame)
        {
            //returns 0 = no player, 1 = 1st player, 2 = 2nd player...
            //bitwise & on firstFrame
            return (int)firstFrame & 7; 
        }


        private int GetDistanceWithPlayerIndex(byte firstFrame, byte secondFrame)
        {
            //offset by 3 in first byte to get value after player index 
            int distance = (int)(firstFrame >> 3 | secondFrame << 5);
            return distance;
        }

        const float MaxDepthDistance = 4000; // max value returned
        const float MinDepthDistance = 850; // min value returned
        const float MaxDepthDistanceOffset = MaxDepthDistance - MinDepthDistance;

        public static byte CalculateIntensityFromDepth(int distance)
        {
            //formula for calculating monochrome intensity for histogram
            return (byte)(255 - (255 * Math.Max(distance - MinDepthDistance, 0) 
                / (MaxDepthDistanceOffset)));
        }

        private void Window_Closed(object sender, EventArgs e)
        {
            //cleanup
            nui.Uninitialize(); 
        }

        }

    }

